00100	IFN USER+TRACK,<0>
00200	STRADR:	MOVE BITSAV	;Enters here every 1/60 th. seccond
00300	IFE USER+TRACK,<
00400		TRNE DTST
00500		CALLI 400024
00600		DATAO ARM,[2]
00700		SOSLE STWT
00800		CALLI 400024
00900	>
01000		MOVE P,[XWD -PDLL,PDL]	;PUSH DOWN STACK
01100		MOVE CBUF,CBUFS
01200		MOVE DATWD,DATWDS
01300		IORI DATWD,2		;YELLOW ARM
01400	IFN REPORT+TRACK,<
01500		MOVE DATA,DATPT
01600	>
01700	IFN USER+TRACK,<
01800		TRNE DTST
01900		JRST SWDONE
02000	>
02100	IFE USER,<
02200	IFN TRACK,<
02300		SKIPE WALK
02400		JRST NCHK
02500	>
02600		CONSZ 40		;CHECK TO SEE IF ON PDP-6
02700		JRST[	MOVEI TAC,PDPERR
02800			MOVEM TAC,TRAJER
02900			JRST TOFF]
03000	;	CONO 4,400		;TURN OFF THE PI'S
03100		CONI 0,AC		;SET PROCESSOR FLAGS
03200		ANDI AC,7
03300		CONO 0,2050(AC)
03400		TRZE TRIP		;TURN ON ARM
03500		DATAO ARM,[3]
03600	>
03700	NCHK:
03800		MOVEI K,51B23		;-10 VOLT REFERENCE
03900	IFE USER,<PUSHJ P,PREAD		;REFTR← -10 volt reference
04000		LDB AC,SNUM		;PICK UP MIDDLE SAMPLE
04100		ANDI DACVAL,7777	;LEAVE LAST SAMPLE IN J
04200		ADDI AC,(DACVAL)	;ADD THEM TOGETHER
04300	>
04400	IFN USER,<MOVEI AC,17770>
04500		CAIGE AC,17000		;IS THE FUSE BLOWN
04600		JRST[		MOVEI TAC,REFERR
04700				MOVEM TAC,TRAJER
04800				JRST TOFF]
04900		MOVEM AC,REFTR		;REFERENCE READING
05000	IFN TRACK,<
05100		SKIPE WALK
05200		JRST TOK
05300	>
05400	IFE USER,<
05500		CONI CLOCK,AC
05600		TLZ AC,777774
05700		JRST STIME		;TIME IN MICRO SECONDS
05800	>
05900	IFN USER,<JRST .+2>
     

00100	NXTJIF:
00200	IFN USER,<JRST SWDONE>
00300	IFN DEB<
00400		MOVEM 17,ACSAV+17
00500		MOVEI 17,ACSAV
00600		BLT 17,ACSAV+16
00700		MOVE 17,ACSAV+17
00800	>
00900	IFN TRACK,<
01000		SKIPE WALK
01100		JRST TOK
01200	>
01300	IFN REPORT+TRACK,<
01400		HRRZI TAC,(DATA)
01500		CAIL TAC,DATO+DBL-1
01600		JRST[	SUB DATA,[XWD DBL,DBL]
01700			MOVE TAC,[XWD DATO+DBL,DATO]
01800			BLT TAC,(DATA)
01900			HRRZI AC,(DATA)
02000			HRRZ TAC,DATIWD
02100			SUBI AC,(TAC)
02200			JRST RDP]
02300		HRRZI AC,(DATA)
02400		HRRZ TAC,DATIWD
02500		SUBI AC,(TAC)
02600		HRRZ MQ,DATPT
02700		SUBI TAC,(MQ)
02800		IMUL AC,TAC
02900	RDP:	SKIPLE AC
03000	IFN TRACK,<PUSH DATA,[0]>
03100	IFN REPORT,<PUSH DATA,[SIXBIT/ERROR/]>
03200		MOVEI AC,DATO-1
03300		HRRZ TAC,DATIWD
03400	 	CAIL TAC,DATO+DBL-1
03500		HRRM AC,DATIWD
03600		MOVEM DATA,DATPT
03700	>
03800	IFE USER,<
03900		MOVEI TAC,=2000
     

00100	WATSTL:	CONI CLOCK,AC
00200		TLZ AC,777774
00300		SUB AC,LTIME
00400		JUMPGE AC,.+2
00500		ADD AC,[=1000000]
00600	IFN REPORT,<
00700		CAIN TAC,=2000
00800		JRST[	PUSH DATA,[<SIXBIT/TICK/>+1]
00900			HRRZ MQ,DATIWD
01000			PUSH DATA,MQ
01100			JRST .+1]
01200	>
01210		MOVEM AC,ETIME#
01220		TRNE RUN
01230		JRST[	SKIPE TP
01240			JRST .+1
01250			MOVE MQ,AC
01260			FSC MQ,233
01270			FMPR MQ,[0.00006]
01280			MOVEM MQ,TDF#
01290			MOVE AC,LTIME
01295			ADD AC,ETIME
01297			JRST SSTIME]
01300		CAIGE AC,=16667		;delta T greater than 16.7 msec.?
01400		SOJGE TAC,WATSTL
01500		JUMPL TAC,TE
01600		CAIL AC,=19000
01700		JRST[	MOVEI TAC,ISERR
01800			MOVEM TAC,TRAJER
01900			JRST TOFF]
01950		MOVSI AC,(1.0)
01975		MOVEM AC,TDF
02000		MOVE AC,LTIME
02100		ADDI AC,=16667
02200	SSTIME:	TLNE AC,777774
02300		SUB AC,[=1000000]
02400	STIME:	MOVEM AC,LTIME		;LTIME←LTIME+16.7 msec.
02500	>
02600		SOSGE COUNT
02700		JRST[TE:MOVEI AC,TERR
02800			MOVEM AC,TRAJER
02900			JRST ZITO]
03000	IFN ADJUST,<
03100		MOVEI K,14B23
03200		MOVEM BITSAV
03300		MOVEI I,2
03400	RNP:	PUSHJ P,PREAD
03500		LDB AC,SNUM
03600		FSC AC,221
03700		JSA 16,EXP
03800		JUMP AC
03900		FMPR [0.15]
04000		MOVEM FACTOR(I)
04100		SOJGE I,RNP
04200		MOVE BITSAV
04300		MOVE I,ADJI
04400		JUMPL I,MAIN
04500	IFN ADJG,<
04600		MOVE AC,ADJE
04700		FMPR AC,FACTOR
04800		MOVEM AC,KE(I)		;POT 12
04900		MOVE AC,ADJV
05000		FMPR AC,FACTOR+1
05100		MOVEM AC,KV(I)		;POT 13
05200		MOVE AC,ADJIF
05300		FMPR AC,FACTOR+2
05400		MOVEM AC,KI(I)	;POT 14
05500	>
05600	IFN ADJD,<
05700		MOVE AC,ADJE
05800		FMPR AC,FACTOR
05900		MOVEM AC,KM(I)		;POT 12
06000		MOVE AC,ADJV
06100		FMPR AC,FACTOR+1
06200		MOVEM AC,PK(I)		;POT 13
06300	>
06400	>
     

00100	MAIN:	PUSHJ P,ANGLES		;read joint angles
00200		TRNE RONLY		;read angles only?
00300		JRST[	PUSHJ P,HEAD	;read hand also
00400			MOVE AC,HAND
00500			MOVEM AC,SET
00600			PUSHJ P,REFLEX
00700			PUSHJ P,READED]	;DOES NOT RETURN
00800		TRNN RUN
00900		JRST[	SKIPGE TAC,FUNCT	;not run, mabye a function
01000			JRST[	PUSHJ P,SETSET
01100				JRST NEXT]
01200			JUMPE TAC,[CAIL CBUF,BUF;try next buffer
01300				SETOM (CBUF)	;dont do this if first buffer
01400				MOVEM CBUF,CBUFS
01500				ADDI CBUF,1001
01600				CAILE CBUF,BUF2
01700				MOVEI CBUF,BUF		;Advance buffer
01800				MOVE 2,(CBUF)
01900				JUMPL 2,[BUFWT:MOVE CBUF,CBUFS;wait for buffer
02000					JRST SWDONE]
02100				JUMPN 2,BUFRDY		;the last buffer stop.
02200				JRST[	TRO DTST
02300			IFN REPORT,<PUSH DATA,[0]>
02400					JRST BUFWT]
02500			BUFRDY:	HRRZ 2,(CBUF)
02600				ADDI 2,-1(CBUF)
02700				HRRZM 2,STKPTR
02800				JRST NEXT]
02900			CAIN TAC,6
03000			JRST SETCH
03100			JRST TT]	;otherwise perform function
03200		MOVEI J,6
03300		PUSHJ P,SERVO		;computes feedback and integral term
03400		JUMPE J,[RUDONE:TRZ DROP+RUN+FINAL+WOB+NUL	;if J is zero then end of run
03450			TDZ DATWD,[770001252500]
03500			PUSHJ P,SETSET
03600			TRZE STP
03700			JRST[	MOVEI TAC,STPERR
03800				MOVEM TAC,TRAJER
03900				POP P,TAC
04000				JRST TOFF]
04100			JRST NEXT]
04200		PUSHJ P,DRIVE		;Drive motors
04300		PUSHJ P,EVAL		;Compute TFF and TDD
04400		PUSHJ P,NXTH		;Compute next T0
     

00100	TT:	PUSHJ P,REFLEX
00200	DISP:	MOVE AC,FUNCT
00300		CAILE AC,MAXFN
00400		JRST NEXT
00500		JRST @.+1(AC)		;Perform function
00600		NXTJIF	;0
00700		HOPEN	;1
00800		HCLOSE	;2
00900		NXTJIF	;3
01000		PLACE	;4
01100		NUDGE	;5
01200		SETCH	;6
01300		STOP	;7
01400		SAVE	;10
01500		RESTORE	;11
01600		CENTER	;12
01700		SET.ARM	;13 SET_ARM
01750		WOBBLE	;14
01800	
01900		XLIST
02000		SUBTTL	SPACEWAR CODE	ANGLE MEASUREMENT	LOU PAUL
02100		LIST
     

00100	ANGLES:	MOVEI I,5
00110		MOVE TAC,[XWD TH,THP]
00120		BLT TAC,THP+5
00200		MOVEI L,10
00300	IFN REPORT,<PUSH DATA,[<SIXBIT/THETA/>+6]>
00400	SETCHA:	MOVEI K,CHA
00500		TRNE ALT6
00600		MOVEI K,N6CHA
00700	RSTRT:	SOJL L,REDER
00800		IFE USER,<
00900		PUSHJ P,PREAD
01000		LDB AC,SNUM
01100		CAIN I,5
01200		JRST[	TRNE POTREAD
01300			JRST .+1
01400			CAIL AC,1000
01500			CAILE AC,7000
01600			JRST[	TRC ALT6
01700				JRST SETCHA]
01800			MOVEI K,CHA5
01900			JRST .+1]
02000		ANDI DACVAL,7777
02100		ADDI AC,(DACVAL)
02200		SETZ TAC,
02300		DIV AC,REFTR
02400		ASH AC,-26
02500		MOVE J,AC
02600		TRNE POTREAD
02700		JRST[	ADDI J,=1000
02800			JRST EXON]
02900		ASHC AC,-11		;LEAVE INDEX INTO NON-LINEAR DIFF TABLE IN AC
03000		LDB MQ,NON(I)		;GET LOWER DIFF
03100		ADDI J,(MQ)		;ADD IT TO READING
03200		AOJ AC,
03300		LDB AC,NON(I)		;GET UPPER DIFFERENCE
03400		SUBI AC,(MQ)
03500		MUL TAC,AC		;INTERPOLATE
03600		ADD J,TAC		;CORRECTED READING
03700	;	COMPUTE TD
03800	IFE TACH,<
03900	BYTACH:	FSC J,226		;FLOAT
04000		FMPR J,SCALE(I)
04100		FADR J,OFF(I)
04200	>
04300	IFN USER,<
04400		MOVE J,ERR(I)
04500		FSC J,-1
04600		FADR J,T0(I)
04700	>
04800		CAIN I,5
04900		JRST[	TRNE ALT6
05000			FADR J,OFF6
05100			FADR J,ROT6
05200			MOVEM J,TH+5
05300			FSBR J,T0+5
05400			MOVSI MQ,(180.0)
05500			CAML J,MQ
05600			JRST[	INCROT:	MOVN MQ,MQ
05700					FSC MQ,1
05800					FADRM MQ,ROT6
05900					FADRM MQ,TH+5
06000					JRST THS]
06100			MOVN MQ,MQ
06200			CAMG J,MQ
06300			JRST INCROT
06400			JRST THS]
06500	EXON:	MOVEM J,TH(I)		;THETA
06700	THS:
06800	IFN REPORT,<
06900		MOVE MQ,TH(I)
07000		FSBR MQ,T0(I)
07100		PUSH DATA,MQ
07200	>
07300	NXCHA:	SOJGE I,RSTRT	; IF LAST TIME THROUGH
     

00100		MOVEI I,5
00200	IFE USER,<
00300	NVEL:	CAIL I,3
00305		JRST[	MOVE AC,TH(I)
00310			FSBR AC,THP(I)
00315			FDVR AC,TDF
00320			JRST MODIFY]
00400		PUSHJ P,PREAD
00500		LDB AC,SNUM
00600		ANDI DACVAL,7777
00700		ADDI AC,(DACVAL)
00800		TRNE VZERO
00900		MOVEM AC,VZ(I)
01000		SUB AC,VZ(I)
01100		FSC AC,216
01200		FMPR AC,VSCALE(I)
01300	IFN TACH,<
01400		TRNE VZERO
01500		SETZM VDTH(I)
01600		FADRM AC,VDTH(I)
01700	>
01800	MODIFY:	MOVEM AC,TD(I)
02600		SOJGE I,NVEL
02700	>
02800		TRZ VZERO
02900		POPJ P,
04000	
04100		XLIST
04200				SUBTTL			SPACEWAR CODE			SERVO CALC.			LOU PAUL
04300		LIST
     

00100	SERVO:	MOVEI I,5
00200		SETZM ASTP
00210		MOVE AC,[XWD ET0,ET0P]
00220		BLT AC,ET0P+5
00300	SERVL:	MOVE TAC,TH(I)
00400		FSBR TAC,T0(I)		;POSITION ERROR
00500		MOVEM TAC,ET0(I)
00600		TRNE FINAL
00700		JRST NULL		;MODIFY THETA BY INTEGRAL TERM
00800	IFE STEP,<
00900		MOVN AC,T0(I)
01000		FSC AC,1
01100		FADR AC,TF(I)
01200		FADR AC,T0P(I)
01300	>
01400	IFN STEP,<SETZ AC,>
01500	MODT0:	MOVE MQ,CII(I)
01600		FMPR AC,MQ
01700		HRRZ K,BMASK(I)
01800		TDNN K,FBI
01900		JRST [	PDIR:SETZM ET0(I)
02200			JRST NOBACK]
02250		FMPR TAC,KI(I)
02260		FMPR MQ,MQ
02270		FDVR TAC,MQ
02300		FADRM TAC,ERRINT(I)
02400	IFE STEP,<
02500		MOVE TAC,T0P(I)
02600		FSBR TAC,T0(I)
02700		FADR TAC,TD(I)
02800	>
02900	IFN STEP,<MOVE TAC,TD(I)>
03000		MOVEM TAC,VERR(I)
03100	
03200		MOVN TAC,VERR(I)
03300		FMPR TAC,KV(I)		;VELOCITY GAIN
03400		FADR AC,TAC
03600		MOVN TAC,ET0(I)
03700		FMPR TAC,KE(I)		;ERROR GAIN
03710		FDVR TAC,CII(I)
03800		FADR AC,TAC
04000	
04200		FADR AC,ERRINT(I)
04300	
04400	NOBACK:	CAIE I,2
04500		FMPR AC,[IRAD:0.0174532925]	;JOINT 3 IS IN INCHES
04800		MOVEM AC,TORE(I)
04900		TRNE STP
05000		JRST[	MOVE MQ,AC
05100			FDVR MQ,F0(I)
05200			SKIPN TD(I)
05300			FSC MQ,-2
05400			FMPR MQ,STQ(I)
05500			FADRM MQ,ASTP
05600			JRST .+1]
05700		FADR AC,CI(I)		;GRAVITY TERM
05800		MOVEM AC,T(I)			;ERROR CORRECTION TORQUE
05900	NXTACC:
06000		SOJGE I,SERVL
06100		TRNE STP
06200		JRST[	MOVE AC,ASTP
06300			CAML AC,SDTQ
06400			JRST[	AOS AC,ASTPP
06500				CAIL AC,3
06600				JRST[	TRZ STP
06700					SUB P,[XWD 1,1]
06710					SETZM ERRINT
06720					MOVE AC,[XWD ERRINT, ERRINT+1]
06730					BLT AC,ERRINT+5
06800					JRST RUDONE]
06900				JRST .+1]
07000			SETZM ASTPP
07100			JRST .+1]
07200	IFN DYNAMICS,<
07300		HRRI TAC,1(DATA)
07400		HRLI TAC,DDAT
07500		HRRZI AC,15(TAC)
07600		BLT TAC,(AC)
07700		ADD DATA,[XWD 16,16]
07800	>
07900		POPJ P,
08000	
08100	NULL:	SETZ AC,
08105		TLNN DATWD,NNUL
08110		TRNE WOB
08120		SOJA J,MODT0
08200		TDNN DATWD,GMASK(I)
08300		SOJA J,NXTACC
08400		MOVE MQ,FBI
08500		TDNN MQ,GMASK(I)
08600		SOJA J,PDIR
08700		MOVM MQ,TAC			;|POSITION ERROR|
08800		CAML MQ,ERR(I)			;COMPARE POSITION ERROR
08900		JRST MODT0
09000		MOVM MQ,ET0P(I)
09100		CAML MQ,ERR(I)
09200		JRST MODT0
09210		MOVE MQ,ET0P(I)
09240		FMPR MQ,TAC
09270		JUMPL MQ,MODT0
09300	SO:	TRNE NUL
09350		SOJA J,MODT0
09375		TDZ DATWD,BMASK(I)
09400	IFN REPORT+TRACK+USER+DEB,<SETZM MOTOR(I)>
09500		DATAO ARM,DATWD
09600		HRLZ MQ,I
09700		TRC MQ,400000
09800		DATAO WIDTH,MQ
09900		SOJA J,NXTACC
10000		XLIST
10100				SUBTTL			SPACEWAR CODE			DRIVE CALC.			LOU PAUL
10200		LIST
     

00100	DRIVE:	JFCL 10,.+1
00200		MOVEI I,5		;Loop once for each joint
00300	IFN REPORT,<PUSH DATA,[<SIXBIT/DAC/>+6]>
00400		TRZ DATWD,504000
00500		TRO DATWD,021200		;INITIAL SIGN BITS FOR POSITIVE ROTATION
00600	LS0:	TDNN DATWD,BMASK(I)
00700		JRST ISP
00800		MOVE MQ,TD(I)
00900		JUMPN MQ,.+2
01000		MOVN MQ,ET0(I)
01100		FMPR MQ,T(I)
01110		TRNE FINAL
01120		JRST[	CAIE I,1
01140			JRST .+1
01150			SETZ MQ,
01160			JRST NOEX]
01200		JUMPL MQ,[	MOVM MQ,T(I)
01300				FSBR MQ,F0(I)
01400				JUMPL MQ,[	FDVR MQ,F0(I)
01500						FMPR MQ,V0(I)
01600						FSC MQ,1
01700						FADR MQ,V0(I)
01800						SKIPGE T(I)
01900						MOVN MQ,MQ
02000						JRST CEMF]
02100				MOVSI AC,(1.0)
02200				FSBR AC,PK(I)
02300				FMPR MQ,AC
02400				FMPR MQ,KM(I)
02500				FADR MQ,V0(I)
02600				SKIPG T(I)
02700				MOVN MQ,MQ
02800				JRST CEMF]
02900		SKIPN T(I)
03000		JRST[		SKIPN TD(I)
03100				JRST CEMF
03200				MOVE MQ,V0(I)
03300				SKIPGE TD(I)
03400				MOVN MQ,MQ
03500				JRST CEMF]
03600		JUMPE MQ,NOEX
03700		MOVE MQ,V0(I)
03800		SKIPGE T(I)
03900		MOVN MQ,MQ
04000	NOEX:	MOVSI AC,(1.0)
04100		FADR AC,PK(I)
04200		FMPR AC,T(I)
04300		FMPR AC,KM(I)
04400		FADR MQ,AC
04500	CEMF:
04600		MOVE AC,EMF(I)
04700		FMPR AC,TD(I)
04800		MOVSI J,(30.0)
04900		JUMPGE MQ,DRVLT
05000		MOVN AC,AC
05100	DRVLT:	FSBR J,AC			;Available drive voltage
05200		CAMGE J,[1.0]
05300		JRST MAXDRV
05400	VELOK:	MOVM AC,MQ
05500		FDVR AC,J		;AC= REL TIME ON
05600		FIX AC,211000		;1=16
05700		CAML AC,[4000000]
05800		JRST DHO		;too much force stop the arm
05900		CAILE AC,776000
06000	MAXDRV:	MOVEI AC,776000
06100	IFN REPORT+TRACK+USER+DEB,<
06200		MOVEM AC,MOTOR(I)
06300		SKIPGE MQ
06400		MOVNS MOTOR(I)
06500	>
06600		TRC AC,400000
06700		HRL AC,I
06800	SETDRV:
06900	IFN TRACK,<SKIPN WALK>
07000		DATAO WIDTH,AC
07100		JUMPGE MQ,ISP
07200		TDC DATWD,DMASK(I)
07300	ISP:
07400	IFN REPORT,<PUSH DATA,MOTOR(I)>
07500		SOJGE I,LS0
07600		JFCL 10,DHO
07700	IFN TRACK,<SKIPN WALK>
07800		DATAO ARM,DATWD
07900		POPJ P,
08000	
08100		XLIST
08200		SUBTTL	SPACEWAR CODE				LOU PAUL
08300		LIST
     

00100	NXTH:	MOVE AC,[XWD T0,T0P]
00200		BLT AC,T0+5
00205		TRNE WOB
00210		JRST[	MOVEI I,2
00215		WL1:	SOSGE J,WOBCNT(I)
00220			JRST[	MOVEI J,=19
00225				MOVEM J,WOBCNT(I)
00230				JRST WL2]
00235		WL2:	MOVE AC,SIN(J)
00240			FMPR AC,WOBMAG
00245			FADRM AC,TFF+3(I)
00250			SOJGE I,WL1
00255			JRST .+1]
00300		TRNE DROP
00400		JRST[	MOVEI I,5
00500		OL:	MOVE AC,DELTH(I)
00550			FMPR AC,TDF
00600			FADRM AC,DTH(I)
00700			SOJGE I,OL
00800			JRST JALS]
00900	NTH:
01000		TRNE INCREM
01100		JRST[	MOVE MQ,ETIME
01150			ADDB MQ,TICKS
01200			CAML MQ,NTICKS
01300			JRST[	TRZ INCREM
01400				SKIPN TP
01500				TRO FINAL
01600				MOVEI AC,=600
01700				MOVEM AC,COUNT
01800				MOVEI I,5
01900			FIL:	MOVE AC,DELTH(I)
02000				FADRB AC,DTH(I)
02100				FADR AC,TFF(I)
02200				MOVEM AC,TF(I)
02300				SETZM DELTH(I)
02400				SOJGE I,FIL
02500				POPJ P,]
02600			SETZ I,
02700			DIV MQ,NTICKS
02800			ASH MQ,-10
02900			FSC MQ,200
03000			MOVSI AC,(6.0)
03100			FMPR AC,MQ
03200			FADR AC,[-15.0]
03300			FMPR AC,MQ
03400			FADR AC,[10.0]
03500			FMPR AC,MQ
03600			FMPR MQ,MQ
03700			FMPR MQ,AC
03800			MOVEI I,5
03900		LDC:	MOVE AC,DELTH(I)
04000			FMPR AC,MQ
04100			FADR AC,DTH(I)
04200			FADR AC,TFF(I)
04300			MOVEM AC,TF(I)
04400			SOJGE I,LDC
04500			POPJ P,]
04600	JALS:	MOVEI I,5
04700	JAL:	MOVE AC,DTH(I)
04800		FADR AC,TFF(I)
04900		MOVEM AC,TF(I)
05000		SOJGE I,JAL
05100		POPJ P,
05200	
     

00100	EVAL:	MOVEI I,5
00200	EL:	MOVE J,TP(I)	;POINTS TO KOE5
00300		JUMPE J,NE
00400		AOS MQ,TJ(I)
00500		CAML MQ,TN(I)	;NUMBER OF TICKS THIS SEGMENT
00600		JRST[				;END OF SEGMENT
00700			CAIN I,5
00800			JRST[			;JOINT 6 HAS CONTROL WORD
00900				MOVE TAC,[XWD NC,CI]
01000				BLT TAC,CI+=11	;UPDATE C
01100				MOVE TAC,TICKS
01200				CAMGE TAC,NTICKS
01300				TRO INCREM
01400				LDB TAC,[POINT 3,1(J),2]	;LEVEL
01500				JUMPN TAC,[			;SWITCH POINT
01600					SKIPG LOOP(TAC)
01700					JRST[			;INITIALIZE LOOP
01800						LDB AC,[POINT 15,1(J),17]
01900						MOVEM AC,LOOP(TAC)	;COUNT
02000						JRST SETALT]
02100					SOSLE LOOP(TAC)		;IN LOOP
02200				SETALT:	TRO ALT
02300					JRST GETNXT]
02400				JRST GETNXT]
02500		GETNXT:	TRNE ALT
02600			JRST[	LDB J,[POINT 9,(J),8]	;ALT POINTER
02700				JRST GOT]
02800			LDB J,[POINT 9,(J),17]		;NEXT POINTER
02900			JUMPE J,[SETZM TP(I)
03000				TRO FINAL
03100				MOVEI AC,=600
03200				MOVEM AC,COUNT
03300				MOVE AC,A4(I)
03400				ADD AC,A3(I)
03500				ADD AC,A2(I)
03600				ADD AC,A1(I)
03700				ADD AC,A0(I)
03800				JRST ELL]
03900		GOT:	ADDI J,(CBUF)
04000			MOVEM J,TP(I)
04100			SETZM TJ(I)
04200			HRRZ AC,(J)
04300			MOVEM AC,TN(I)
04400			CAIN I,5
04500			JRST[
04600				HRRZ TAC,1(J)
04700				ADDI TAC,(CBUF)
04800				MOVEI MQ,14(TAC)
04900				HRLI TAC,(TAC)
05000				HRRI TAC,NC
05100				BLT TAC,NC+=11
05200				MOVEI K,5
05300			AAI:	MOVE TAC,MOTARM(K)
05400				FADRM TAC,NC+6(K)
05500				SOJGE K,AAI
05600				MOVEI K,5
05700				MOVE MQ,(MQ)
05800			FTF:	MOVE TAC,GMASK(K)
05900				TDNN TAC,FBI
06000				JRST[	TDNN TAC,MQ
06100					JRST FNF
06200					MOVE AC,TH(K)
06300					MOVEM AC,T0(K)
06400					FADR AC,TD(K)
06500					MOVEM AC,TF(K)
06600					FSBR AC,TFF(K)
06700					MOVEM AC,DTH(K)
06800					MOVNM AC,DELTH(K)
06900					MOVE AC,TN+5
06950					IMULI AC,=16667
07000					MOVEM AC,NTICKS
07100					SETZM TICKS
07200					TRO INCREM
07300					JRST FNF]
07400				TDNN TAC,MQ
07500				JRST[	MOVE AC,T0(K)
07600					FSC AC,1
07700					FSBR AC,T0P(K)
07800					FSBR AC,TF(K)
07900					CAIE K,2
08000					FMPR AC,IRAD
08100					FMPR AC,CII(K)
08200					FADR AC,T(K)
08300					MOVEM AC,CI(K)
08400					JRST FNF]
08500			FNF:	SOJGE K,FTF
08600				HRRZM MQ,FBI
08700				MOVEI K,=11
08800				MOVE AC,TN+5
08900				FSC AC,233
09000				MOVSI MQ,(1.0)
09100				FDVR MQ,AC
09200			DELC:	MOVE AC,NC(K)
09300				FSBR AC,CI(K)
09400				FMPR AC,MQ
09500				MOVEM AC,DCI(K)
09600				SOJGE K,DELC
09700				JRST DDELC]
09800		DDELC:	HLRE AC,-3(J)
09900			MOVEM AC,A4(I)
10000			HRRE AC,-3(J)
10100			MOVEM AC,A3(I)
10200			HLRE AC,-2(J)
10300			MOVEM AC,A2(I)
10400			HRRE AC,-2(J)
10500			MOVEM AC,A1(I)
10600			HLLZ AC,-1(J)
10700			MOVEM AC,SC(I)
10800			HRRE AC,-1(J)
10900			MOVEM AC,A0(I)
11000			JRST ELL]
11100		CAIN I,5
11200		JRST[	MOVEI K,=11
11300		DECC:	MOVE AC,DCI(K)
11400			FADRM AC,CI(K)
11500			SOJGE K,DECC
11600			JRST .+1]
11700		DIV MQ,TN(I)
11800		MOVE AC,A4(I)
11900		MUL AC,MQ
12000		ADD AC,A3(I)
12100		MUL AC,MQ
12200		ADD AC,A2(I)
12300		MUL AC,MQ
12400		ADD AC,A1(I)
12500		MUL AC,MQ
12600		ADD AC,A0(I)
12700	ELL:	XOR AC,SC(I)
12800		FSC AC,0
12900		MOVEM AC,TFF(I)
13000		CAIN I,5
13100		JRST[	MOVE AC,TFF+5
13200			FSBR AC,T0+5
13300			MOVSI MQ,(180.0)
13400			CAML AC,MQ
13500			JRST[	INCT0:	FSC MQ,1
13600					FADRM MQ,T0+5
13700					FADRM MQ,TF+5
13800					JRST .+1]
13900			MOVN MQ,MQ
14000			CAMG AC,MQ
14100			JRST INCT0
14200			JRST .+1]
14300	NE:	SOJGE I,EL
14400		TRZ ALT
14500		POPJ P,
14600	
     

00100	DHO:	AOJ I,			;DISASTER HAS OCCURED
00200		LSH I,3			;JOINT NUMBER
00300		IORI I,EXER
00400		MOVEM I,TRAJER
00500		POP P,I
00600	ZITO:	SETZM ERRINT
00700		MOVE AC,[XWD ERRINT,ERRINT+1]
00800		BLT AC,ERRINT+5
00900	
01000	TOFF:	MOVE AC,STKPTR
01100		MOVEM AC,ERRSTK
01200		MOVEI L,1000
01300		TRNE RONLY
01400		JRST READED
01500		TRZ RUN+FINAL+DROP+INCREM+HCL+STP+WOB+NUL	;STOP THE ARM
01600		SETZM TP
01700		MOVE AC,[XWD TP,TP+1]
01800		BLT AC,TP+5
01900		PUSHJ P,SETSET
02000	MOVNXT:	AOS AC,STKPTR
02100	MOVNX:	SOJL L,[ LE:
02200			MOVEI TAC,LOOPER
02300			MOVEM TAC,TRAJER
02400			JRST PW]
02500		HLRE TAC,@STKPTR
02600		CAIN TAC,3		;IS IT A WAIT
02700		JRST MOVNXT
02800		CAIN TAC,SKPERR
02900		JRST[	HRRZ TAC,(AC)
03000			CAMN TAC,TRAJER
03100			JRST[	AOS STKPTR
03200				JRST NEXT]
03300			JRST MOVNXT]
03400		CAIN TAC,SKPNER
03500		JRST[	HRRZ TAC,(AC)
03600			CAME TAC,TRAJER
03700			JRST[	AOS STKPTR
03800				JRST MOVNXT]
03900			JRST NEXT]
04000		CAIN TAC,SKPSER
04100		JRST[	LDB MQ,[POINT 3,(AC),35]
04200			LDB TAC,[POINT 3,TRAJER,35]
04300			CAIN TAC,(MQ)
04400			JRST[	HRRZ TAC,(AC)
04500				ANDI TAC,777770
04600				TDNE TAC,TRAJER
04700				JRST[	AOS STKPTR
04800					JRST NEXT]
04900				JRST MOVNXT]
05000			JRST MOVNXT]
05100		CAIN TAC,JMPREL
05200		JRST[	HRRE TAC,(AC)
05300			ADDI AC,(TAC)
05400			HRRM AC,STKPTR
05500			JRST MOVNX]
05600	PW:	CAIL AC,BUF
05700		HRRM AC,JUMPBK
05800		MOVEI AC,WAITIN
05900		MOVEM AC,STKPTR
06000		MOVEI AC,3
06100		MOVEM AC,FUNCT
06200	IFN TRACK,<SKIPN WALK>
06300		DATAO ARM,[2]		;STOP THE ARM
06400	IFN REPORT,<PUSH DATA,[0]>
06500	READED:	TRO DTST
06600	IFN DEB,<
06700		MOVEM 17,ACSAV+17
06800		MOVEI 17,ACSAV
06900		BLT 17,ACSAV+16
07000	>
07100		JRST SWDONE
07200	
     

00100	NEXT:	TRNE RUN
00200		JRST[	SETZM FUNCT
00300			JRST NXTJIF]
00400		MOVEI L,1000
00500		MOVE AC,FUNCT
00600		CAIE AC,6		;SET TOUCH
00700		JRST[	MOVE TAC,SOTCHS
00800			MOVEM TAC,SOTCH
00900			TRZ STCH
01000			JUMPE TAC,.+1
01100			TRO STCH
01200			JRST .+1]
01300	IFN TRACK,<SKIPN WALK>
01400		MOVEI DATWD,2
01500		DATAO ARM,DATWD		;STOP THE ARM
01600	INCPIC:	AOS AC,STKPTR
01700	PICKUP:	SOJL L,LE
01800		HLRE TAC,@STKPTR
01900		CAIN TAC,JMPABS
02000		JRST[JMPA:HRRZ TAC,@STKPTR
02100			MOVEM TAC,STKPTR
02200			JRST PICKUP]
02300		CAIN TAC,SKPERR
02400		JRST[	HRRZ TAC,@STKPTR
02500			CAMN TAC,TRAJER
02600			AOS STKPTR
02700			JRST INCPIC]
02800		CAIN TAC,SKPNER
02900		JRST[	HRRZ TAC,@STKPTR
03000			CAME TAC,TRAJER
03100			AOS STKPTR
03200			JRST INCPIC]
03300		CAIN TAC,SKPSER
03400		JRST[	LDB MQ,[POINT 3,@STKPTR,35]
03500			LDB TAC,[POINT 3,TRAJER,35]
03600			CAIN TAC,(MQ)
03700			JRST[	HRRZ TAC,@STKPTR
03800				ANDI TAC,777770
03900				TDNE TAC,TRAJER
04000				AOS STKPTR
04100				JRST INCPIC]
04200			JRST INCPIC]
04300		CAIN TAC,JMPREL
04400		JRST[	HRRE TAC,@STKPTR
04500			ADD TAC,STKPTR
04600			HRRM TAC,STKPTR
04700			JRST PICKUP]
04800		SETZM TRAJER
04900		JUMPL TAC,[TRO RUN+VZERO
05000			SETCA TAC,
05100			MOVEM TAC,FUNCT
05200			AOS DOING		;INCREASE TRAJECTORY NUMBER
05300			HLRZ 2,(CBUF)
05350			IMULI 2,=16667
05400			MOVEM 2,NTICKS		;RUN TIME
05500			MOVEI 2,=1800
05600			MOVEM 2,COUNT
05700			SETZM LOOP
05800			MOVE TAC,[XWD LOOP,LOOP+1]
05900			BLT TAC,TN+5
06000			MOVE DATWD,777(CBUF)
06100			HRRZM DATWD,FBI
06200			MOVEI I,5
06300			MOVEI K,34
06400		SUL:	SUBI K,4
06500			HRLZM K,TJ(I)
06600			MOVEI TAC,TJ(I)
06700			MOVEM TAC,TP(I)
06800			TDNE DATWD,BMASK(I)
06900			TDO DATWD,BMASK(I)
07000			SETZM DTH(I)
07100			SOJGE I,SUL
07200	
07300			HRLZI TAC,763(CBUF)
07400			HRRI TAC,NC
07500			BLT TAC,NC+=11
07600			MOVEI I,5
07700		SAAI:	MOVE TAC,MOTARM(I)
07800			FADRM TAC,NC+6(I)
07900			SOJGE I,SAAI
08000			PUSHJ P,EVAL
08100			TRZ INCREM
08200			MOVEI I,5
08300		MOVBK:	MOVE AC,TH(I)
08400			MOVEM AC,T0(I)
08500			MOVEM AC,TF(I)
08600			FSBR AC,TFF(I)
08700			CAIN I,5
08800			JRST[	MOVSI MQ,(180.0)
08900				CAML AC,MQ
09000				JRST[SETROT:FSC MQ,1
09100					MOVN MQ,MQ
09200					FADRM MQ,TH+5
09300					FADRM MQ,ROT6
09400					JRST MOVBK]
09500				MOVN MQ,MQ
09600				CAMG AC,MQ
09700				JRST SETROT
09800				JRST ADDTF]
09900		ADDTF:	MOVEM AC,DTH(I)
10000			MOVN AC,AC
10100			FADRM AC,DELTH(I)
10200			SETZM TD(I)
10300			SOJGE I,MOVBK
10400			PUSHJ P,EVAL
10500			PUSHJ P,NXTH
10600			SETZM TICKS
10700			HLRE TAC,@STKPTR
10800			JRST MARK]
10900		MOVEM TAC,FUNCT
11000		CAIN TAC,3		;IS IT A WAIT
11100		JRST[	AOS TAC,STKPTR
11200			CAIL TAC,BUF
11300			HRRM TAC,JUMPBK
11400			MOVEI TAC,WAITIN
11500			MOVEM TAC,STKPTR
11600	IFN REPORT,<PUSH DATA,[0]>
11700			TRO DTST
11800			PUSHJ P,SWDONE]
11900	MARK:
12000	IFN REPORT,<
12100		PUSH DATA,[<SIXBIT/NEXT/>+1]
12200		MOVE TAC,FUNCT
12300		HRL TAC,0
12400		PUSH DATA,TAC
12500	>
12600	
12700	;	JRST DISP
12800	SWDONE:
12900		MOVEM BITSAV
13000		MOVEM DATWD,DATWDS
13100		MOVEM CBUF,CBUFS
13200	IFN REPORT+TRACK,<
13300		MOVEM DATA,DATPT
13400	>
13500	IFE USER,<
13600	IFN TRACK,<
13700		SKIPE WALK
13800		JRST @STRADR-1
13900	>
14000	;	CONO 4,200		;TURN ON THE PI'S
14100		CALLI 400024
14200	>
14300	IFN USER,<
14400		JRST @STRADR-1
14500	>
14600	
14700	SETSET:	MOVEI I,5
14800	TCHK:	MOVE J,TP(I)
14900		JUMPE J,ATEND
15000		SETZM TJ(I)
15100		SETZM TN(I)
15200	GNN:	LDB J,[POINT 9,(J),17]
15300		ADDI J,(CBUF)
15400		CAIN J,(CBUF)
15500		JRST[	MOVEM J,TP(I)
15600			JRST ATEND]
15700		JRST GNN
15800	ATEND:	SOJGE I,TCHK
15900		PUSHJ P,EVAL
16000		MOVEI I,5
16100	FDTH:	MOVE AC,TH(I)
16200		MOVEM AC,T0(I)
16300		MOVEM AC,T0P(I)
16400		MOVEM AC,TF(I)
16500		FSBR AC,TFF(I)
16600		MOVEM AC,DTH(I)
16700		SETZM DELTH(I)
16800		SOJGE I,FDTH
16900		POPJ P,
17000	
     

00100	PREAD:
00200	IFN TRACK,<
00300		SKIPE WALK
00400		JRST[GNPR:AOBJP DATA,[	MOVE AC,[IOWD DBL,DATO]
00500					MOVEM AC,DATPT
00600					MOVE DATA,DATPT
00700					SETZ TAC,
00800					INPUT 15,1
00900					JRST GNPR]
01000			MOVE DACVAL,(DATA)
01100			JRST @READFN]
01200	>
01300		MOVEI TAC,10
01400		MOVEM TAC,DATMSD
01500	DACRED:	SOSGE DATMSD
01600		JRST[REDER:	MOVEI TAC,READER	
01700				MOVEM TAC,TRAJER
01800				JRST TOFF]
01900	DACST:	CONO DB,4250
02000		CONO AD,(K)
02100	NREAD:	MOVEI TAC,30
02200		CONSO DB,1000
02300		SOJGE TAC,.-1
02400		JUMPL TAC,[DACDIE:CONO AD,4000(K)	;STOP DAC
02500				TRO REREAD
02600			IFN DEB,<MOVEM DACVAL,DACERR>
02700				MOVEI TAC,12
02800			WRH:	SOJGE TAC,WRH
02900				JRST DACRED]
03000		DATAI DB,DACVAL
03100		CONSZ DB,10000
03200		JRST DACDIE
03300		XOR DACVAL,SBMSK
03400	IFN TRACK,<PUSH DATA,DACVAL>
03500		JRST @READFN
03600	FREAD:	ADDI K,770000
03700	IFN TRACK,<SKIPN WALK>
03800		CONO AD,4000(K)	;STOP AD ON NEXT CHANNEL
03900		POPJ P,
04000	READFN:	FREAD
04100	
04200		XLIST
04300		SUBTTL	CONSTANTS				LOU PAUL
04400		LIST
     

00100	NON:	POINT 12,INNER(AC),11
00200		POINT 12,INNER(AC),23
00300		POINT 12,INNER(AC),35
00400		POINT 12,OUTER(AC),11
00500		POINT 12,OUTER(AC),23
00600		POINT 12,OUTER(AC),35
00700	ERR:	0.05
00800		0.05
00900		0.01
01000		0.1
01100		0.1
01200		0.7
01300	TH:	BLOCK 6		;THE NEXT 7 LOCATIONS ARE BLOCK TRANSFERED.
01400	HAND:	3.0
01500	VERR:	BLOCK 6
01700	ERRINT:	0
01800		0
01900		0
02000		0
02100		0
02200		0
02300	DTH:	BLOCK 6
02400	TD:	BLOCK 6
02410	ET0P:	BLOCK 6
02420	ET0:	BLOCK 6
02500	T0P:	BLOCK 6
02600	T0:	-150.0
02700		-75.0
02800		14.0
02900		-90.0
03000		 60.0
03100		 0.0000
03200	TF:	BLOCK 6
03300	TFF:	-150.0
03400		-75.0
03500		14.0
03600		-90.0
03700		 60.0
03800		 0.0000
03900	DELTH:	BLOCK 6		;THIS AND THE NEXT ARE BLT ED TOGETHER
04000	NTICKS:	0
04100	DTHS:	BLOCK =60
04200	TORE:	BLOCK 6
04300	T:	BLOCK 6
04400	IFN USER+REPORT+TRACK+DEB,<MOTOR:	BLOCK 6>
04500	FBI:	0
04600	VZ:	BLOCK 3
04700	IFN TACH,<
04900	VDTH:	BLOCK 3
05000	>
05100	THP:	BLOCK 6
05200	MOTARM:	700000.0
05300		800000.0
05400		5000.0
05500		3000.0
05600		3000.0
05700		4000.0
05800	STQ:	BLOCK 6
05900	SDTQ:	0
06000	ASTP:	0
06100	ASTPP:	0
06200	DCI:	BLOCK 6
06300	DCII:	BLOCK 6
06400	CI:	0
06500		0
06600		0
06700		0
06800		0
06900		0
07000	CII:	500000.0
07100		700000.0
07200		 10000.0
07300		100000.0
07400		100000.0
07500		  1000.0
07600	NC:	BLOCK =12
07700	IFN DEB,<DACERR:	0>
07800	REFTR:	0
07900	IFN DEB,<
08000	ETIM:	0
08100	ACSAV:	BLOCK 20
08200	>
08300	PDL:	DHO
08400		BLOCK PDLL
08500	LTIME:	0
08600	FUNCT:	0
08700	ROT6:	0
08800	TRAJER:	0
08900	DOING:	0
09000	DATMSD:	0
09100	CBUFS:	0
09200	DATWDS:	0
09300	SNUM:	POINT 12,DACVAL,23
09400	SBMSK:	400040004000
09500	DMASK:	400000
09600		100000
09700		20000
09800		4000
09900		1000
10000		200
10100	BMASK:	XWD 400000,200000
10200		XWD 200000, 40000
10300		XWD 100000, 10000
10400		XWD  40000,  2000
10500		XWD  20000,   400
10600		XWD  10000,   100
10700	GMASK:	200000
10800		 40000
10900		 10000
11000		  2000
11100		   400
11200		   100
11300	GAIN:	MOVEI I,5
11309	SETGN:	MOVE AC,KV(I)
11318		FMPR AC,AC
11327		FSC AC,-2
11336		MOVEM AC,KE(I)
11345		FMPR AC,KV(I)
11354		FSC AC,-4
11363		MOVNM AC,KI(I)
11372		SOJGE I,SETGN
11373		MOVE 1,[SIXBIT/$ARM$/]
11377		CALLI 1,400002
11381		MOVE I,RET
11385		MOVEM I,GAIN
11387		JRST GAIN
11388	RET:	POPJ P,
11394		BLOCK 11
11400	WAITIN:	XWD 3,0
11500	JUMPBK:	XWD JMPABS,0
11600	STKPTR:	WAITIN
11700	ERRSTK:	0
11800	BITSAV:	0
11900	COUNT:	0
12000	STWT:	0
12100	HCNT:	0
12200	TP:	0
12300		0
12400		0
12500		0
12600		0
12700		0
12800	TJ:	BLOCK 6
12900	LOOP:	BLOCK 10
13000	TN:	BLOCK 6
13100	SC:	BLOCK 6
13200	A0:	BLOCK 6
13300	A1:	BLOCK 6
13400	A2:	BLOCK 6
13500	A3:	BLOCK 6
13600	A4:	BLOCK 6
13700	BLOCKS:	0
13800	NXTBUF:	0
13900	BUF:	0
14000		BLOCK 1000
14100	BUF2:	0
14200		BLOCK 1000
14300	IFN ADJUST<
14400	ADJI:	-1
14500	ADJE:	0
14600	ADJV:	0
14700	ADJIF:	0
14800	FACTOR:	BLOCK 3
14900	>
15000	IFN REPORT+TRACK,<
15100	DATIWD:	0
15200		0
15300	DATPT:	0
15400	DATO:	BLOCK DBL+100
15500	>
15600	IFN DYNAMICS,<
15700	DDAT:	<SIXBIT/FORD/>+6
15800	FOR:	BLOCK 6
15900		<SIXBIT/BACK/>+6
16000	BAK:	BLOCK 6
16100	>